#include "bulletBridge.h"
#define SIMD_PI_2 ((SIMD_PI)*0.5f)

bulletBridge::bulletBridge( btDynamicsWorld* world, Ogre::SceneManager *mgr, Ogre::Vector3 bbsize )
{
	m_world=world;
	int seg = 20;
	len = 105;
	Ogre::Vector3 curpos(-10, 10, 0);
	btCollisionShape* shape = new btBoxShape(btVector3(bbsize.x,bbsize.y,bbsize.z));

	btTransform tr;
	tr.setIdentity();
	tr.setOrigin(btVector3(curpos.x, curpos.y, curpos.z));
	tr.getBasis().setEulerZYX(0,0,0);
	btRigidBody* pBodyA = localCreateRigidBody( 0.0, tr, shape);
	m_BgRigidbodys.push_back(pBodyA);
	pBodyA->setActivationState(DISABLE_DEACTIVATION);
	btRigidBody* pBodyB;
	btTransform frameInA, frameInB;
	frameInA = btTransform::getIdentity();
	frameInA.setOrigin(btVector3(btScalar(1.), btScalar(0.), btScalar(0.)));
	frameInB = btTransform::getIdentity();
	frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.)));
	
	btGeneric6DofSpringConstraint* pGen6DOFSpring;

	for (float x=-10; x<10; x++)
	{
		curpos.x += bbsize.x+1;

		tr.setIdentity();
		tr.setOrigin(btVector3(curpos.x, curpos.y, curpos.z ));
		tr.getBasis().setEulerZYX(0,0,0);
		pBodyB = localCreateRigidBody(1.0, tr, shape);
		pBodyB->setActivationState(DISABLE_DEACTIVATION);
		m_BgRigidbodys.push_back(pBodyB);


		pGen6DOFSpring = new 
			btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
		pGen6DOFSpring->setLinearUpperLimit(btVector3(1., 1., 1.));
		pGen6DOFSpring->setLinearLowerLimit(btVector3(-1., -1., -1.));

		pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, 0));
		pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 0));

		m_world->addConstraint(pGen6DOFSpring, true);
		pGen6DOFSpring->setDbgDrawSize(btScalar(5.f));

		pGen6DOFSpring->enableSpring(0, true);
		pGen6DOFSpring->setStiffness(0, 39.478f);
		pGen6DOFSpring->setDamping(0, .05f);
		pBodyA = pBodyB;
	}
	curpos.x += bbsize.x+1;

	tr.setIdentity();
	tr.setOrigin(btVector3(curpos.x, curpos.y, curpos.z ));
	tr.getBasis().setEulerZYX(0,0,0);
	pBodyB = localCreateRigidBody(0.0, tr, shape);
	pBodyB->setActivationState(DISABLE_DEACTIVATION);
	m_BgRigidbodys.push_back(pBodyB);

	pGen6DOFSpring = new 
		btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true);
	pGen6DOFSpring->setLinearUpperLimit(btVector3(1., 1., 1.));
	pGen6DOFSpring->setLinearLowerLimit(btVector3(-1., -1., -1.));

	pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, 0));
	pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 0));

	m_world->addConstraint(pGen6DOFSpring, true);
	pGen6DOFSpring->setDbgDrawSize(btScalar(5.f));

	pGen6DOFSpring->enableSpring(0, true);
	pGen6DOFSpring->setStiffness(0, 39.478f);
	pGen6DOFSpring->setDamping(0, .05f);
}

btRigidBody* bulletBridge::localCreateRigidBody( float mass, 
										   const btTransform& startTransform, btCollisionShape* shape )
{
	btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		shape->calculateLocalInertia(mass,localInertia);
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects

	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia);
	btRigidBody* body = new btRigidBody(cInfo);
	m_world->addRigidBody(body);

	return body;
}